#include "robot_locomotion.h"

using namespace std;


#define TIMESTEP 0.01


rl::rl(void)
{
    //变量初始化
    cout << "初始化开始" << endl;
    state = 0;
    count = 0;
    set_flag = 0;
    read_flag = 0;

    x_pre = 0;
    y_pre = 0;
    theta_pre = 0;

    vx = 0;
    vy = 0;
    w = 0;

    k1 = 3;
    c1 = 3.6;
    k2 = 0.00724;
    c2 = 0.058;
    // k1 = 8;
    // c1 = 6.4;
    // k2 = 0.00724;
    // c2 = 0.058;
    BL = 0.3;
    BW = 0.16;
    M = 3;
    I = 1.0 / 12 * M * (BL*BL + BW*BW);

    dsensor[0] = 0;
    dsensor[1] = 0;
    dsensor[2] = 0;
    dsensor[3] = 0;
    dsensor[4] = 0;
    dsensor[5] = 0;
 

}
void rl::getvalue()
{
    dsensor[0] = 0;
    dsensor[1] = 0;
    dsensor[2] = 0;
    dsensor[3] = 0;
    dsensor[4] = 0;
    dsensor[5] = 0;

}

void rl::run()
{   
    //用于速度控制
    
    switch (state)
    {   
        case 0:
            vx = 0;
            vy = 0;
            w = 0;
            break;

        //直行
        case 1:
            cout << "case1 1号舱道直行" << endl;
            //前进方向速度
            vx = 0.04;
            lside_MSD(0.14);
            if (count > 0)
            {
                vy = 0;
                w = 0;
                //surmount_flag = 1;
            }
            break;

        //turn
        case 2:
            cout << "case2 1号舱道turn" << endl;
            //前进方向速度
            vx = -0.005;
            vy = -0.01;
            w = 0.1;
            break;

        //surmount
        case 3:
            cout << "case3 1号舱道locate" << endl;
            //前进方向速度
            k1 = 8;
            c1 = 6.4;
        
            rear_MSD(0.07);
            rside_MSD(0.14,0);
            break;

        case 4:
            cout << "case4 1号舱道surmount" << endl;
            set_flag = 1;
            break;

        case 5:
            cout << "case5 steady..." << endl;
            vy = 0;        
            rear_MSD(0.25);
            break;

        case 6:
            cout << "case6 1号舱道surmount part2" << endl;
            set_flag = 1;
            break;
        
        case 7:
            cout << "case7 steady..." << endl;
            front_MSD(0.08);
            break;
        
        case 8:
            cout << "case8 2号舱道turn" << endl;
            //前进方向速度
            vx = -0.005;
            vy = -0.01;
            w = 0.1;
            break;

        case 9:
            cout << "case9 2号舱道直行" << endl;
            //前进方向速度
            vx = 0.04;
            rside_MSD(0.14,1);
            break;
    }

}

void rl::update()
{
    switch (state)
    {
        //触发条件
        case 0:
            cout << "case 0 num" << count << endl;
            count++;
            if (count > 400)
            {
                count = 0;
                state = 1;
            }
            break;

        case 1:
            //连续检测到孔位，超过阈值次数达到5次
            if (dsensor[0] > 0.5)
            {
                count++;
            }

            if (count > 270)
            {
                count = 0;
                state = 2;
            }
            break;
        
        case 2:
            //后端检测距离连续小于阈值超过三次
            if ((dsensor[2] + dsensor[3] < 0.3) && (abs(dsensor[2] - dsensor[3]) < 0.003))
            {
                count++;
            }
            
            if (count > 20)
            {
                count = 0;
                state = 3;
            }
            break;
        
        case 3:
            if ((abs(dsensor[2] - dsensor[3]) < 0.01) && (abs(dsensor[4] - dsensor[5]) < 0.01))
            {
                count++;
            }
            cout << "count " << count << endl;
            if (count > 200)
            {
                count = 0;
                state = 4;
            }
            break;

        case 4:
            if (read_flag == 1)
            {
                set_flag = 0;
                state = 5;
            }
            // if (dsensor[2] > 0.45)
            // {
            //     set_flag = 0;
            //     state = 5;
            // }
            break;

        case 5:
            if (abs(dsensor[6] - dsensor[7]) < 0.001)
            {
                count++;
            }
            if (count > 50)
            {
                count = 0;
                state = 6;
            }
            break;
        
        case 6:
            if (read_flag == 1)
            {
                set_flag = 0;
                state = 7;
            }
            break;
        
        case 7:
            if (abs(dsensor[6] - dsensor[7]) < 0.01)
            {
                count++;
            }
            if (count > 500)
            {
                count = 0;
                state = 8;
            }
            break;
        
        case 8:
            if (abs(dsensor[4] - dsensor[5]) < 0.01)
            {
                count++;
            }
            
            if (count > 200)
            {
                count = 0;
                state = 9;
            }
            break;
        
        
    }
}

void rl::sendvalue()
{
    // velx = vx;
    // vely = vy;
    // velw = w;
}

void rl::rside_MSD(double threshold,int theta_on)
{   
    y = ((dsensor[4]+dsensor[5])/2 - threshold);
    //vy = (y - y_pre)/TIMESTEP;
    y_pre = y;
    ay = 1/M * (-k1*y-c1*vy);
    vy = vy + ay * TIMESTEP;

    if (theta_on == 1)
    {
        theta = atan2(dsensor[4]-dsensor[5],BL);
        cout << "theta" << theta << endl;
        //w = (theta - theta_pre)/TIMESTEP;
        theta_pre = theta;
        cout << "I" << I << endl;
        alpha = (-k2*theta-c2*w)/I;
        cout << "alpha" << alpha << endl;
        w = w + alpha * TIMESTEP;
        cout << "w" << w << endl;
    }


    // vy = 0;
    // w = 0;
}

void rl::lside_MSD(double threshold)
{   
    y = -((dsensor[0]+dsensor[1])/2 - threshold);
    //vy = (y - y_pre)/TIMESTEP;
    y_pre = y;
    ay = 1/M * (-k1*y-c1*vy);
    vy = vy + ay * TIMESTEP;

    theta = atan2(dsensor[1]-dsensor[0],BL);
    cout << "theta" << theta << endl;
    //w = (theta - theta_pre)/TIMESTEP;
    theta_pre = theta;
    cout << "I" << I << endl;
    alpha = (-k2*theta-c2*w)/I;
    cout << "alpha" << alpha << endl;
    w = w + alpha * TIMESTEP;
    cout << "w" << w << endl;

    // vy = 0;
    // w = 0;
}

void rl::rear_MSD(double threshold)
{   
    x = ((dsensor[2]+dsensor[3])/2 - threshold);
    //vy = (y - y_pre)/TIMESTEP;
    x_pre = x;
    ax = 1/M * (-k1*x-c1*vx);
    vx = vx + ax * TIMESTEP;

    theta = atan2(dsensor[3]-dsensor[2],BW);
    cout << "theta" << theta << endl;
    //w = (theta - theta_pre)/TIMESTEP;
    theta_pre = theta;
    cout << "I" << I << endl;
    alpha = (-k2*theta-c2*w)/I;
    cout << "alpha" << alpha << endl;
    w = w + alpha * TIMESTEP;
    cout << "w" << w << endl;

    // vy = 0;
    // w = 0;
}

void rl::front_MSD(double threshold)
{   
    x = ((dsensor[6]+dsensor[7])/2 - threshold);
    //vy = (y - y_pre)/TIMESTEP;
    x_pre = x;
    ax = 1/M * (-k1*x-c1*vx);
    vx = vx + ax * TIMESTEP;

    theta = atan2(dsensor[7]-dsensor[6],BW);
    cout << "theta" << theta << endl;
    //w = (theta - theta_pre)/TIMESTEP;
    theta_pre = theta;
    cout << "I" << I << endl;
    alpha = (-k2*theta-c2*w)/I;
    cout << "alpha" << alpha << endl;
    w = w + alpha * TIMESTEP;
    cout << "w" << w << endl;

    // vy = 0;
    // w = 0;
}